#include "Map.h"
#include "MapMaker.h"
#include "KeyFrame.h"
#include <fstream>
#include <iostream>
#include <cvd/image_io.h>
#include <boost/filesystem.hpp>

Map::Map()
{
  Reset();
}

Map::~Map()
{
  Reset();
}

void Map::Reset()
{
  boost::unique_lock< boost::shared_mutex > lockMapPoints(mMapPointsLock);
  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mKeyFramesLock);
  boost::unique_lock< boost::shared_mutex > lockMesh(mMeshLock);

  mrtAllKeyFrames.RemoveAll();
  msLocalOptimKeyFrames.clear();
  msLocalFixedKeyFrames.clear();

  mvAllMapPoints.clear();
  msLocalMapPoints.clear();

  mesh.Reset();

  mpFirstKeyFrame.reset();

  vSnapshotPoses.clear();

  bGood = false;

  lockKeyFrames.unlock();
  lockMapPoints.unlock();
  lockMesh.unlock();
}

/*void Map::Merge(Map *pMapSection)
{
  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mKeyFramesLock);
  boost::unique_lock< boost::shared_mutex > lockMapPoints(mMapPointsLock);

  Vector<3> mv3MapWorldOriginShift = mv3MapWorldOrigin - pMapSection->mv3MapWorldOrigin;

  // Merge global keyframes, local keyframes are ignored
  std::cout << "Merge Size: " << pMapSection->mrtAllKeyFrames.Size() << " " << pMapSection->mvAllMapPoints.size() << std::endl;

  std::cout << "Inserting global keyframes" << std::endl;
  RTree<KeyFrame, double, 3>::Iterator itree;
  pMapSection->mrtAllKeyFrames.GetFirst(itree);
  while(!pMapSection->mrtAllKeyFrames.IsNull(itree))
  {
    boost::shared_ptr<KeyFrame> pNewKF = pMapSection->mrtAllKeyFrames.GetAt(itree);
    pNewKF->mse3GfromC.get_translation() += mv3MapWorldOriginShift;

    SE3<> se3WfromC = pNewKF->se3CfromW.inverse();
    se3WfromC.get_translation() += mv3MapWorldOriginShift;
    pNewKF->se3CfromW = se3WfromC.inverse();

    mrtAllKeyFrames.Insert(se3WfromC.get_translation().get_data_ptr(), pNewKF);
    pMapSection->mrtAllKeyFrames.GetNext(itree);
  }
  pMapSection->mrtAllKeyFrames.RemoveAll();
  pMapSection->msLocalOptimKeyFrames.clear();
  pMapSection->msLocalFixedKeyFrames.clear();

  // Merge global mappoints, local mappoints are ignored
  std::cout << "Inserting global mappoints" << std::endl;
  for(std::vector<boost::shared_ptr<MapPoint> >::iterator it = pMapSection->mvAllMapPoints.begin(); it != pMapSection->mvAllMapPoints.end(); ++it)
  {
    (*it)->v3WorldPos += mv3MapWorldOriginShift;
	  (*it)->RefreshPixelVectors();
  }
  mvAllMapPoints.insert(mvAllMapPoints.end(), pMapSection->mvAllMapPoints.begin(), pMapSection->mvAllMapPoints.end());
  pMapSection->mvAllMapPoints.clear();
  pMapSection->msLocalMapPoints.clear();

  std::cout << "Completed map merge" << std::endl;

  lockMapPoints.unlock();
  lockKeyFrames.unlock();
}*/

void Map::Save()
{
  boost::unique_lock< boost::shared_mutex > lockMapPoints(mMapPointsLock);
  boost::unique_lock< boost::shared_mutex > lockKeyFrames(mKeyFramesLock);

  std::cout << "  Mapper: Saving the map.... " << std::endl;
  boost::filesystem::create_directory("map");
  std::ofstream ofs("map/mappoints.data");
  for(unsigned int i = 0; i < mvAllMapPoints.size(); i++)
  {
    if(!mvAllMapPoints[i]->bBad)
    {
      ofs << mvAllMapPoints[i]->v3WorldPos << "  ";

      // Normal of point is approximated by averaging optical axis directions of keyframes in which point is visible
      Vector<3> v3Normal = makeVector(0, 0, 0);
      unsigned int nCount = 0;
      for(std::set<boost::shared_ptr<KeyFrame> >::iterator it = mvAllMapPoints[i]->sMeasurementKFs.begin(); it != mvAllMapPoints[i]->sMeasurementKFs.end(); ++it)
      {
        v3Normal += unit((*it)->se3CfromW.inverse().get_translation() - mvAllMapPoints[i]->v3WorldPos);
        nCount++;
      }
      v3Normal /= nCount;

      ofs << v3Normal << " ";
      ofs << mvAllMapPoints[i]->sMeasurementKFs.size() << std::endl; //Number of keyframes in which mappoint is observed
    }
  }
  ofs.close();

  RTree<KeyFrame, double, 3>::Iterator itree;
  mrtAllKeyFrames.GetFirst(itree);
  unsigned int i = 0;
  std::ofstream ofs2("map/keyframes.data");
  std::filebuf fb;

  while(!mrtAllKeyFrames.IsNull(itree))
  {
    boost::shared_ptr<KeyFrame> kf = mrtAllKeyFrames.GetAt(itree);

    std::ostringstream imname;
    imname << "map/keyframe" << i++ << ".pnm";
    fb.open (imname.str().c_str(),std::ios::out);
    std::ostream os(&fb);
    pnm_save(kf->image, os);
    fb.close();

    ofs2 << imname.str() << " ";
    ofs2 << kf->se3CfromW << std::endl;

    mrtAllKeyFrames.GetNext(itree);
  }
  ofs2.close();

  std::ofstream ofs3("map/snapshots.data");
  for(unsigned int i = 0; i < vSnapshotPoses.size(); i++)
    ofs3 << vSnapshotPoses[i].first << std::endl << vSnapshotPoses[i].second << std::endl;
  ofs3.close();

  std::ofstream ofs4("map/descriptors.data");
  for(unsigned int i = 0; i < mvAllMapPoints.size(); i++)
  {
    if(!mvAllMapPoints[i]->bBad)
    {
      for(int j = 0; j < 64; ++j)
        ofs4 << (int)mvAllMapPoints[i]->pDescriptor[j] << " ";
      ofs4 << std::endl;
    }
  }
  ofs4.close();

  lockMapPoints.unlock();
  lockKeyFrames.unlock();

  std::cout << "  ... done saving map." << std::endl;
}


